/**
 * @(#) EPuck.java
 */

package pcsr.drivers.epuckDriver;

import java.util.ArrayList;
import java.util.Collection;
import java.util.Hashtable;
import java.util.LinkedList;
import java.util.List;

import pcsr.drivers.epuckDriver.EpuckMotorActuator.EpuckMotorSide;
import pcsr.drivers.epuckDriver.command.EPuckCommand;
import pcsr.drivers.epuckDriver.command.ISensorStatusObserver;
import pcsr.drivers.epuckDriver.command.IePuckCommand;
import pcsr.drivers.epuckDriver.command.IePuckUartProtocol.LedNumber;
import pcsr.drivers.epuckDriver.command.IePuckUartProtocol.LedStatus;
import pcsr.drivers.epuckDriver.command.IePuckUartProtocol.ProximityNumber;
import pcsr.hwVirtualization.Actuator;
import pcsr.hwVirtualization.ActuatorMode;
import pcsr.hwVirtualization.ActuatorValue;
import pcsr.hwVirtualization.IRobotDriverObservable;
import pcsr.hwVirtualization.IRobotDriverObserver;
import pcsr.hwVirtualization.RobotInfo;
import pcsr.hwVirtualization.Sensor;
import pcsr.hwVirtualization.SensorMode;
import pcsr.hwVirtualization.SensorValue;


public class EPuck implements IRobotDriverObservable, ISensorStatusObserver
{
	private static final int ActuatorMaxValue = 500;
	private static IePuckCommand epuckCommand = EPuckCommand.getSingleton();
	
	private static final int SensorMaxValue = 500;
	private EpuckAccelerometerSensor accelerometer;
	
	private Hashtable<String, IEpuckActuator> actuators;	
	private EpuckLightActuator bodyLed;

	private List<IRobotDriverObserver> epuckObservers = new LinkedList<IRobotDriverObserver>();	
	private EpuckLightActuator frontLed;
	private EpuckMotorActuator leftMotor;
	private EpuckSoundSensor microphone;
	private EpuckLightActuator perimeterLed_01;
	private EpuckLightActuator perimeterLed_02;
	private EpuckLightActuator perimeterLed_03;
	private EpuckLightActuator perimeterLed_04;
	private EpuckLightActuator perimeterLed_05;
	private EpuckLightActuator perimeterLed_06;	
	private EpuckLightActuator perimeterLed_07;
	private EpuckLightActuator perimeterLed_08;
	private EpuckProximitySensor perimeterSensor_01;

	private EpuckProximitySensor perimeterSensor_02;
	private EpuckProximitySensor perimeterSensor_03;
	private EpuckProximitySensor perimeterSensor_04;
	private EpuckProximitySensor perimeterSensor_05;
	private EpuckProximitySensor perimeterSensor_06;
	private EpuckProximitySensor perimeterSensor_07;
	private EpuckProximitySensor perimeterSensor_08;
	private EpuckMotorActuator rightMotor;
	private Hashtable<String, IEpuckSensor> sensors;	
	private EpuckSoundActuator speaker;
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public Collection<Actuator> GetAllActuators() {
		Collection<Actuator> ret = new LinkedList<Actuator>();
		
		for(IEpuckActuator na : actuators.values())
		{
			Actuator a = TranslateActuator(na);
			ret.add(a);
		}
		return ret;
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public Collection<Sensor> GetAllSensors() {
		Collection<Sensor> ret = new LinkedList<Sensor>();
		
		for(IEpuckSensor ns : sensors.values())
		{
			Sensor s = TranslateSensor(ns);
			ret.add(s);
		}
		return ret;
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public RobotInfo GetRobotInfo() {
		RobotInfo ret = new RobotInfo();
		String robotName = epuckCommand.getDeviceName();
		if(robotName == null || robotName.length() < 1)
		{
			ret.SetName("E-Puck Robot");
		}
		else
		{
			ret.SetName(robotName);
		}
		return ret;
	}

	/**
	 * {@inheritDoc}
	 */
	@Override
	public void ObserverRobotDriver(IRobotDriverObserver observer) {
		epuckObservers.add(observer);
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public void SetActuatorStatus(Actuator actuator) {
		IEpuckActuator theEpuckActuator = actuators.get(actuator.GetActuatorID());
		if(theEpuckActuator != null)
		{			
			theEpuckActuator.SetValue((short) actuator.GetActuatorValue().GetUnnormalizedValue());
		}
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public void SetSensorStatus(Sensor sensor) {
		// TODO Auto-generated method stub
		
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public void UnObserveRobotDriver(IRobotDriverObserver observer) {
		epuckObservers.remove(observer);
	}
	
	/**
	 * {@inheritDoc}
	 */
	@Override
	public void updateSensor(EpuckSensorType st, int[] args) {
		switch (st) {
		case PERIMETER:
			//System.out.println("PROXIMITY : " + args[0] + " , " + args[1] + " , " + args[2] + " , " + args[3] + " , " + args[4] + " , " + args[5] + " , " + args[6] + " , " + args[7]);
			perimeterSensor_01.SetValue((short) args[0]);
			perimeterSensor_02.SetValue((short) args[1]);
			perimeterSensor_03.SetValue((short) args[2]);
			perimeterSensor_04.SetValue((short) args[3]);
			perimeterSensor_05.SetValue((short) args[4]);
			perimeterSensor_06.SetValue((short) args[5]);
			perimeterSensor_07.SetValue((short) args[6]);
			perimeterSensor_08.SetValue((short) args[7]);
			NotifySensorObservers(perimeterSensor_01);
			NotifySensorObservers(perimeterSensor_02);
			NotifySensorObservers(perimeterSensor_03);
			NotifySensorObservers(perimeterSensor_04);
			NotifySensorObservers(perimeterSensor_05);
			NotifySensorObservers(perimeterSensor_06);
			NotifySensorObservers(perimeterSensor_07);
			NotifySensorObservers(perimeterSensor_08);
			break;
		case MOCROPHONE:
			//System.out.println("MOCROPHONE : " + args[0] + " , " + args[1] + " , " + args[2] );
			microphone.SetX((short) args[0]);
			microphone.SetY((short) args[1]);
			microphone.SetZ((short) args[2]);
			NotifySensorObservers(microphone);
			break;
		case ACCELEROMETER:
			//System.out.println("ACCELEROMETER : " + args[0] + " , " + args[1] + " , " + args[2] );
			accelerometer.SetX((short) args[0]);
			accelerometer.SetY((short) args[1]);
			accelerometer.SetZ((short) args[2]);
			NotifySensorObservers(accelerometer);
			break;
		case SPEED:
			leftMotor.SetValue((short) args[0]);
			rightMotor.SetValue((short) args[1]);
			break;
		case POSITION:
			leftMotor.SetPosition((short) args[0]);
			rightMotor.SetPosition((short) args[1]);
			break;
		default:
			break;
		}
	}
	
	/**
	 * {@inheritDoc}
	 */
	public void Initialize() {};
	
	/**
	 * Initializes an instance of EPuck driver class.
	 */
	private void InitializeInternal(){
		leftMotor = new EpuckMotorActuator(this, EpuckMotorSide.LEFT);		
		rightMotor = new EpuckMotorActuator(this, EpuckMotorSide.RIGHT);		
		perimeterLed_01 = new EpuckLightActuator(this, LedNumber.LED_1);
		perimeterLed_02 = new EpuckLightActuator(this, LedNumber.LED_2);
		perimeterLed_03 = new EpuckLightActuator(this, LedNumber.LED_3);
		perimeterLed_04 = new EpuckLightActuator(this, LedNumber.LED_4);
		perimeterLed_05 = new EpuckLightActuator(this, LedNumber.LED_5);
		perimeterLed_06 = new EpuckLightActuator(this, LedNumber.LED_6);
		perimeterLed_07 = new EpuckLightActuator(this, LedNumber.LED_7);
		perimeterLed_08 = new EpuckLightActuator(this, LedNumber.LED_8);
		bodyLed = new EpuckLightActuator(this, LedNumber.BODY);
		frontLed = new EpuckLightActuator(this, LedNumber.FRONT);
		speaker = new EpuckSoundActuator(this);
		
		actuators.put(leftMotor.GetID(), leftMotor);
		actuators.put(rightMotor.GetID(), rightMotor);
		actuators.put(perimeterLed_01.GetID(), perimeterLed_01);
		actuators.put(perimeterLed_02.GetID(), perimeterLed_02);
		actuators.put(perimeterLed_03.GetID(), perimeterLed_03);
		actuators.put(perimeterLed_04.GetID(), perimeterLed_04);
		actuators.put(perimeterLed_05.GetID(), perimeterLed_05);
		actuators.put(perimeterLed_06.GetID(), perimeterLed_06);
		actuators.put(perimeterLed_07.GetID(), perimeterLed_07);
		actuators.put(perimeterLed_08.GetID(), perimeterLed_08);
		actuators.put(bodyLed.GetID(), perimeterLed_01);
		actuators.put(frontLed.GetID(), perimeterLed_01);
		actuators.put(speaker.GetID(), speaker);
		
		perimeterSensor_01 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_1);
		perimeterSensor_02 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_2);
		perimeterSensor_03 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_3);
		perimeterSensor_04 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_4);
		perimeterSensor_05 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_5);
		perimeterSensor_06 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_6);
		perimeterSensor_07 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_7);
		perimeterSensor_08 = new EpuckProximitySensor(ProximityNumber.PROXIMITY_8);		
		accelerometer = new EpuckAccelerometerSensor();		
		microphone = new EpuckSoundSensor();
		
		sensors.put(perimeterLed_01.GetID(), perimeterSensor_01);
		sensors.put(perimeterLed_02.GetID(), perimeterSensor_02);
		sensors.put(perimeterLed_03.GetID(), perimeterSensor_03);
		sensors.put(perimeterLed_04.GetID(), perimeterSensor_04);
		sensors.put(perimeterLed_05.GetID(), perimeterSensor_05);
		sensors.put(perimeterLed_06.GetID(), perimeterSensor_06);
		sensors.put(perimeterLed_07.GetID(), perimeterSensor_07);
		sensors.put(perimeterLed_08.GetID(), perimeterSensor_08);
		sensors.put(accelerometer.GetID(), accelerometer);
		sensors.put(microphone.GetID(), microphone);
		
		epuckCommand.observeSensors(this);
		
		epuckCommand.open();
	}
	
	/**
	 * Notify observers that a change occurred in a sensor. 
	 * @param sensor The sensor that changed.
	 */
	private void NotifySensorObservers( IEpuckSensor sensor) {
		for(IRobotDriverObserver ro : epuckObservers)
		{
			ro.SensorChanged(TranslateSensor(sensor));
		}
	}
	
	/**
	 * Translates an EPuck actuator to a PCSR actuator representation.
	 * @param na The EPuck actuator.
	 * @return The PCSR actuator.
	 */
	private Actuator TranslateActuator(IEpuckActuator na) {
		Actuator a = new Actuator(na.GetID());	
		
		ActuatorValue av = new ActuatorValue(ActuatorMaxValue);
		double avv = na.GetValue();
		av.SetUnnormalizedValue(avv);
		a.SetValue(av);
		
		ActuatorMode am = new ActuatorMode();		
		ArrayList<String> availModes = new ArrayList<String>();
		availModes.add(na.GetMode().name());
		am.SetAvailableModes(availModes);		
		am.SetSelectedMode(na.GetMode().name());
		a.SetMode(am);
		
		return a;
	}
	
	/**
	 * Translates an EPuck sensor to a PCSR sensor representation.
	 * @param ns The EPuck sensor.
	 * @return The PCSR sensor.
	 */
	private Sensor TranslateSensor(IEpuckSensor ns) {
		Sensor s = new Sensor(ns.GetID());
		
		SensorMode sm = new SensorMode();		
		ArrayList<String> availModes = new ArrayList<String>();		
		availModes.add(ns.GetMode().name());		
		sm.SetAvailableModes(availModes);
		sm.SetSelectedMode(ns.GetMode().name());	
		s.SetMode(sm);
		
		SensorValue sv = new SensorValue(SensorMaxValue);
		double svv = ns.GetValue();
		sv.SetUnnormalizedValue(svv);
		s.SetValue(sv);
		
		return s;
	}
	
	/**
	 * Sends a command in order to set the EPuck actuator state into hardware.
	 * @param actuator The EPuck representation of the actuator.
	 */
	void SendActuatorUpdateCommand(IEpuckActuator actuator)
	{
		switch (actuator.GetType()) {
		case LIGHT:
			EpuckLightActuator la = (EpuckLightActuator)actuator;			
			epuckCommand.setLed(la.GetLedNumber(), LedStatus.GetByValue(((actuator.GetValue() > 0) ? 1 : 0)));
			break;
		case MOTOR:
			epuckCommand.setMotorPosition(leftMotor.GetPosition(), rightMotor.GetPosition());
			epuckCommand.setMotorSpeed(leftMotor.GetSpeed(), rightMotor.GetSpeed());
			break;
		case SOUND:
			EpuckSoundActuator sa = (EpuckSoundActuator) actuator;
			epuckCommand.PlaySound(sa.GetSound());
			break;
		default:
			break;
		}
	}
	
	{
		actuators = new Hashtable<String, IEpuckActuator>();
		sensors = new Hashtable<String, IEpuckSensor>();
		InitializeInternal();
	}
}
